#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTServo,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     Ultra,          sensorSONAR)
#pragma config(Sensor, S4,     HTSMUX,         sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     B_Left,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     F_Left,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     Lift,          tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     Scooper,       tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     Launcher,      tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_2,     Raise,         tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C3_1,     B_Right,       tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C3_2,     F_Right,       tmotorTetrix, PIDControl, encoder)
#pragma config(Servo,  srvo_S2_C1_1,    Hook,                 tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    Guide,                tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////


#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "AutonomousBase.c"	//Use Autnomous Functions With Include Files

#pragma DebuggerWindows("joystickGame");


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
  servoTarget[Hook] = 50;  // 50 is up 245 is down
  //motor[Scooper] = 0;
  //servoTarget[Guide] = 237;

  return;
}

int anLookup[] = {
	// We've noticed that the joystick values can be up to 128!
	-80, -70, -50, -30, -20, -10, -5, 0, 0, 5, 10, 20, 30, 50, 70, 80, 80};

// motor smoother, makes motor values change slower(?)
int motorSmoother(int before, int after){
	int nSmooth = after - before;
	if (nSmooth < 0){
		if (nSmooth < -12) {
			nSmooth = -12;
		}
	}
	else
	{
		if (nSmooth > 12) {
			nSmooth = 12;
		}
	}

	return before + nSmooth;
 }




int convertJoystick2Motor(int y){
	if ((y < -128) || (y > 128)) {
		writeDebugStreamLine("Problem with input!  y=%d", y);
	}
	return (anLookup[(y + 128) / 16]);

}




/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
//   1. Loop forever repeating the following actions:
//   2. Get the latest game controller / joystick settings that have been received from the PC.
//   3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
//      simple action:
//      *  Joystick values are usually directly translated into power levels for a motor or
//         position of a servo.
//      *  Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//         position.
//   4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

task main()
{
  initializeRobot();

  waitForStart();   // wait for start of tele-op phase

  int launch_v, launch_prev, thresh = 20, mmult = 1;
	int current_BL, current_BR, current_FL, current_FR;
	int X1_1C, X2_1C, Y1_1C, Y2_1C, Y1_2C, Y2_2C;
	int X1_1, X2_1, Y1_1, Y2_1, Y2_2, Y1_2;
	int raiseST, liftST;

  while (true)
  {
  	 current_FR = motor[F_Right];
	   current_FL = motor[F_Left];
	   current_BR = motor[B_Right];
	   current_BL = motor[B_Left];

	// setting the motor values in relation to the joystick values
		getJoystickSettings(joystick);
		 X1_1C = joystick.joy1_x1;
		 X2_1C = -joystick.joy1_x2;
		 Y1_1C = joystick.joy1_y1;
		 Y2_1C = joystick.joy1_y2;
		 Y1_2C = joystick.joy2_y1;
		 Y2_2C = joystick.joy2_y2;



	/*sets motor values to 0 if they're values are below 20; this is to give the joysticks
	 a dead zone so they don't always have values*/
		if(abs(X1_1C) < thresh)
			X1_1C = 0;
		if(abs(X2_1C) < thresh)
			X2_1C = 0;
		if(abs(Y1_1C) < thresh)
			Y1_1C = 0;
		if(abs(Y1_2C) < thresh)
			Y1_2C = 0;
		if(abs(Y2_1C) < thresh)
			Y2_1C = 0;
		if(abs(Y2_2C) < thresh)
			Y2_2C = 0;
		if(abs(Y1_2C) < thresh)
			Y1_2C = 0;


		if(abs(X1_1C) > 20 || abs(Y1_1C) > 20)//First Qualifier Config
		{
			X2_1C = 0;
		}


		 X1_1 = convertJoystick2Motor(X1_1C);
		 X2_1 = convertJoystick2Motor(X2_1C);
     Y1_1 = convertJoystick2Motor(Y1_1C);
		 Y2_1 = convertJoystick2Motor(Y2_1C);
		 Y2_2 = convertJoystick2Motor(Y2_2C) / 15;
		 Y1_2 = convertJoystick2Motor(Y1_2C) / 36;


	  int F_R = (-Y1_1 - X1_1 + X2_1);	//First Qualifier config
	  int F_L = ( Y1_1 - X1_1 + X2_1);
	  int B_R = (-Y1_1 + X1_1 + X2_1);
	  int B_L = ( Y1_1 + X1_1 + X2_1);

	  motor[F_Left] = F_L;
	  motor[F_Right] = F_R;
	  motor[B_Right] = B_R;
	  motor[B_Left] = B_L;

	  if(joy1Btn(3) == 1 && X1_1 == 0 && X2_1 == 0 && Y1_1 == 0 && Y2_1 == 0)
	  {
	  	mmult = -mmult;
			wait10Msec(25);
		}

		/*if(joy1Btn(5) == 0 && joy1Btn(6) == 0)
		{
	   motor[F_Right] =  motorSmoother(current_FR, (-Y1_1 * mmult));
	   motor[B_Right] =  motorSmoother(current_BR, (-Y1_1 * mmult));
	   motor[F_Left]  =  motorSmoother(current_FL, (Y2_1 * mmult));
	   motor[B_Left] =   motorSmoother(current_BL, (Y2_1 * mmult));

	  }*/


	  liftST = TSreadState(BtnLI);
	  raiseST = TSreadState(BtnRA);

	  /*if(liftST == 1 && Y2_2 > 0 && joy2Btn(10) == 0)
	  		Y2_2 = 0;*/
	  	//writeDebugStreamLine("Encoder Lift = %d", nMotorEncoder[Lift]);

	  //motor[Lift] = Y2_2;

		if(joystick.joy2_TopHat == 0 && raiseST == 0)
			motor[Raise] = -15;
		if(joystick.joy2_TopHat != 0 || raiseST == 1)
			motor[Raise] = 0;

	 	motor[Scooper] = -Y1_2;
	/*if(joystick.joy1_TopHat == 0)
	{servoTarget[Guide] += 1; wait10Msec(50);}	For calibrating guide
	if(joystick.joy1_TopHat == 4)
	{servoTarget[Guide] -= 1; wait10Msec(50);}*/
	//writeDebugStreamLine("Guide = %d Scoop = %d", servoTarget[Guide], motor[Scooper]);
	if(joy2Btn(1) == 1)
		servoTarget[Guide] = 221;
	if(joy2Btn(4) == 1)
		servoTarget[Guide] = 232;
	if(joy2Btn(1) == 0 && joy2Btn(4) == 0)
		servoTarget[Guide] = 237;
	writeDebugStreamLine("button 8 = %d  hook = %d", joy1Btn(8), servoTarget[Hook]);

	if(joy1Btn(8) == 1)// button 8 is the right trigger
   {
    servoTarget[Hook] = 245;	// hooke(1) is the preset "down" position
  }
  /*else if (joy1Btn(6) == 1 && X1_1 == 0 && X2_1 == 0 && Y1_1 == 0 && Y2_1 == 0 ) // button 6 is the right bumper
   {
		motor[F_Right] = motorSmoother(current_FR, -50);
		motor[B_Right] = motorSmoother(current_BR, 50);  //Regionals translation config
		motor[F_Left] = motorSmoother(current_FL, -50);
		motor[B_Left] = motorSmoother(current_BL, 50);
   }*/
   if (joy1Btn (7) == 1) // button 7 is the left trigger
  {
   servoTarget[Hook] = 50;// hook(0) is the preset "up" position
  }
   /*else if (joy1Btn(5) == 1 && X1_1 == 0 && X2_1 == 0 && Y1_1 == 0 && Y2_1 == 0)// button 5 is the left bumper
   {
    motor[F_Right] = motorSmoother(current_FR, 50);
		motor[B_Right] = motorSmoother(current_BR, -50);
		motor[F_Left] = motorSmoother(current_FL, 50);
		motor[B_Left] = motorSmoother(current_BL, -50);
  }*/

  if(joy2Btn(6) == 1 && raiseST == 1)
  {
  	launch_v = -80;
  	ClearTimer(T4);
  }
  if(joy2Btn(5) == 1 && time10[T4] > 200)
  	launch_v = 5;
 	else if(joy2Btn(5) == 0 && joy2Btn(6) == 0)
 		launch_v = 0;

 		launch_prev = motor[Launcher];
 		motor[Launcher] = motorSmootherL(launch_prev, launch_v);







    // Look in the ROBOTC samples folder for programs that may be similar to what you want to perform.
    // You may be able to find "snippets" of code that are similar to the functions that you want to
    // perform.
  }
}
